#!/usr/bin/env python3

import rospy
from sensor_msgs.msg import Joy
from geometry_msgs.msg import TwistStamped
import yaml
import os
from controller_manager_msgs.srv import SwitchController, SwitchControllerRequest
from pghl_claw.pghl_con import ClawController
import threading


class XboxController:
    def __init__(self):
        rospy.init_node('xbox_servo_control', anonymous=True)

        # 加载参数
        self.load_params()

        # 初始化速度指令发布器
        self.velocity_pub = rospy.Publisher(self.velocity_topic, TwistStamped, queue_size=10)

        # 初始化速度指令
        self.twist_msg = TwistStamped()
        self.twist_msg.header.frame_id = self.frame_id

        # 控制频率
        self.rate = rospy.Rate(self.control_rate)  # 根据 YAML 文件设置频率

        # 切换控制器
        self.switch_controllers()

        # 初始化夹爪控制
        if self.gripper_con:
            self.claw_server = ClawController()
            self.claw_initialized = self.claw_server.initialize_claw()
            if not self.claw_initialized:
                rospy.logerr("夹爪初始化失败！")
        else:
            self.claw_initialized = False

        # 锁定机制，防止频繁触发夹爪控制
        self.gripper_lock = threading.Lock()

        # 按键状态
        self.prev_lt_value = 0
        self.prev_rt_value = 0

        # 订阅Xbox手柄消息
        rospy.Subscriber("/joy", Joy, self.joy_callback)

    def load_params(self):
        """从参数服务器加载参数"""
        # 加载安全保护参数
        self.max_linear_speed = rospy.get_param("~max_linear_speed", 0.1)  # 默认值为 0.1
        self.max_angular_speed = rospy.get_param("~max_angular_speed", 0.5)  # 默认值为 0.5
        self.dead_zone = rospy.get_param("~dead_zone", 0.1)  # 默认值为 0.1

        # 加载 ROS 主题和帧名称
        self.velocity_topic = rospy.get_param("~velocity_topic", "/panda_link8/velocity")  # 默认值为 /panda_link8/velocity
        self.frame_id = rospy.get_param("~frame_id", "panda_link0")  # 默认值为 panda_link0

        # 加载控制频率
        self.control_rate = rospy.get_param("~control_rate", 10)  # 默认值为 10 Hz

        # 加载手柄轴映射
        self.axes_map = rospy.get_param("~axes_map", {
            'left_stick_x': 0,
            'left_stick_y': 1,
            'right_stick_x': 3,
            'right_stick_y': 4,
            'left_trigger': 2,  # 左扳机键
            'right_trigger': 5   # 右扳机键
        })

        # 加载按键映射
        self.buttons_map = rospy.get_param("~buttons_map", {
            'button_a': 0,
            'button_b': 1,
            'button_x': 2,
            'button_y': 3
        })

        # 加载控制器切换参数
        self.start_controllers = rospy.get_param("~start_controllers", "joint_group_velocity_controller").split(',')
        self.stop_controllers = rospy.get_param("~stop_controllers", "position_joint_trajectory_controller").split(',')
        self.strictness = rospy.get_param("~strictness", SwitchControllerRequest.STRICT)

        # 加载夹爪控制参数
        self.gripper_con = rospy.get_param("~gripper_con", False)  # 是否启用夹爪控制
        self.open_position = rospy.get_param("~open_position", 8000)  # 夹爪打开位置
        self.close_position = rospy.get_param("~close_position", 6000)  # 夹爪闭合位置
        self.force_value = rospy.get_param("~force_value", 100)  # 夹爪夹取力

    def switch_controllers(self):
        """切换控制器：关闭 stop_controllers，打开 start_controllers"""
        try:
            # 等待服务可用
            rospy.wait_for_service('/controller_manager/switch_controller', timeout=5)
            switch_controller_srv = rospy.ServiceProxy('/controller_manager/switch_controller', SwitchController)

            # 调用服务
            con = SwitchControllerRequest()
            con.strictness= self.strictness
            con.start_controllers = ["joint_group_velocity_controller"]
            con.stop_controllers  = ["position_joint_trajectory_controller"]
            response = switch_controller_srv(con)

            if response.ok:
                rospy.loginfo("Controllers switched successfully.")
            else:
                rospy.logwarn("Failed to switch controllers.")

        except rospy.ServiceException as e:
            rospy.logerr(f"Service call failed: {e}")
        except rospy.ROSException as e:
            rospy.logerr(f"Service not available: {e}")

    def joy_callback(self, data):
        """处理手柄输入"""
        # 获取手柄轴数据
        left_stick_x = data.axes[self.axes_map['left_stick_x']]
        left_stick_y = data.axes[self.axes_map['left_stick_y']]
        right_stick_x = data.axes[self.axes_map['right_stick_x']]
        right_stick_y = data.axes[self.axes_map['right_stick_y']]
        lt_value = data.axes[self.axes_map['left_trigger']]
        rt_value = data.axes[self.axes_map['right_trigger']]

        # 获取按键状态
        button_a = data.buttons[self.buttons_map['button_a']]  # Z 轴正向移动
        button_b = data.buttons[self.buttons_map['button_b']]  # Z 轴反向移动
        button_x = data.buttons[self.buttons_map['button_x']]  # 绕 Z 轴正向旋转
        button_y = data.buttons[self.buttons_map['button_y']]  # 绕 Z 轴反向旋转

        # 应用手柄死区
        left_stick_x = self.apply_dead_zone(left_stick_x)
        left_stick_y = self.apply_dead_zone(left_stick_y)
        right_stick_x = self.apply_dead_zone(right_stick_x)
        right_stick_y = self.apply_dead_zone(right_stick_y)

        # 设置线性速度指令（左摇杆）
        self.twist_msg.twist.linear.x = -left_stick_y * self.max_linear_speed  # 前后平移
        self.twist_msg.twist.linear.y = left_stick_x * self.max_linear_speed   # 左右平移
        self.twist_msg.twist.linear.z = (button_a - button_b) * self.max_linear_speed  # Z 轴移动

        # 设置角速度指令（右摇杆 + 按键）
        self.twist_msg.twist.angular.x = -right_stick_y * self.max_angular_speed  # 绕 X 轴旋转（右摇杆 Y 轴）
        self.twist_msg.twist.angular.y = -right_stick_x * self.max_angular_speed  # 绕 Y 轴旋转（右摇杆 X 轴）
        self.twist_msg.twist.angular.z = (button_x - button_y) * self.max_angular_speed  # 绕 Z 轴旋转（按键）

        # 发布速度指令
        self.publish_velocity()

        # 处理夹爪控制按键
        if self.gripper_con and self.claw_initialized:
            self.handle_gripper_buttons(lt_value, rt_value)

    def apply_dead_zone(self, value):
        """应用手柄死区"""
        if abs(value) < self.dead_zone:
            return 0.0
        else:
            return value

    def handle_gripper_buttons(self, button_open, button_close):
        """处理夹爪控制按键"""
        if button_open == 1:  # 按下按钮 X 打开夹爪
            with self.gripper_lock:
                if not self.claw_server.set_gripper(self.open_position):
                    rospy.logerr("打开夹爪失败！")

        elif button_close == 1:  # 按下按钮 Y 闭合夹爪
            with self.gripper_lock:
                if not self.claw_server.set_gripper(self.close_position):
                    rospy.logerr("闭合夹爪失败！")

    def publish_velocity(self):
        """发布速度指令"""
        self.twist_msg.header.stamp = rospy.Time.now()
        self.velocity_pub.publish(self.twist_msg)

    def handle_gripper_axes(self, lt, rt):
        """处理夹爪控制轴"""
        if lt > 0.5:  # LT 按键按下超过一半时，打开夹爪
            with self.gripper_lock:
                if not self.claw_server.set_gripper(self.open_position, self.force_value):
                    rospy.logerr("打开夹爪失败！")

        elif rt > 0.5:  # RT 按键按下超过一半时，闭合夹爪
            with self.gripper_lock:
                if not self.claw_server.set_gripper(self.close_position, self.force_value):
                    rospy.logerr("闭合夹爪失败！")

    def run(self):
        """主循环"""
        while not rospy.is_shutdown():
            self.rate.sleep()

if __name__ == '__main__':
    try:
        controller = XboxController()
        controller.run()
    except rospy.ROSInterruptException:
        pass